#include "ParticleFilter.h"
#include <cstdlib>
#include <random>
#include <numeric>
#include "config.h"
#include "MPointCloudMap.h"
#include "config.h"
#define TIME_USING to_string((int)(end_time-start_time)*1000/CLOCKS_PER_SEC)

ParticleFilter::ParticleFilter()
{
	
}


ParticleFilter::~ParticleFilter()
{

}

void ParticleFilter::setCallback(showPredictionParticles showPredictionParticles, 
	bool* has_add,
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr prediction_particles,
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer_pt)
{
	m_showPredictionParticles = showPredictionParticles;
	m_has_add = has_add;
	m_prediction_particles = prediction_particles;
	m_viewer_pt = viewer_pt;
}

void ParticleFilter::predictionDone(ParticlesData::Particles& shift_particles)
{
	if (m_showPredictionParticles) {
		if (shift_particles.size() != 0) {
			m_showPredictionParticles(m_has_add, getParticlesAsPointCloud(shift_particles,m_prediction_particles), m_viewer_pt);
		}	
		else
			m_showPredictionParticles(m_has_add,getParticlesAsPointCloud(m_prediction_particles),m_viewer_pt);
	}
}



void ParticleFilter::particleDataInit(const vector<Pose3D>& ground_truth)
{

	if (ground_truth.size() == 0)
		return;
	//init the mean pose
	init_x = ground_truth[0].x;
	init_y = ground_truth[0].y;
	init_z = ground_truth[0].z;
	init_yaw = ground_truth[0].yaw;

	//the uniform distribution paragram
	double max_x{ 0.0 }, min_x{ 0.0 };
	double max_y{ 0.0 }, min_y{ 0.0 };
	double max_z{ 0.0 }, min_z{ 0.0 };

	max_x = ground_truth[0].x + mPF_option.init_area_xy;
	max_y = ground_truth[0].y + mPF_option.init_area_xy;
	max_z = ground_truth[0].z + mPF_option.init_area_z;
	min_x = ground_truth[0].x - mPF_option.init_area_xy;
	min_y = ground_truth[0].y - mPF_option.init_area_xy;
	min_z = ground_truth[0].z - mPF_option.init_area_z;
	mInitPara.particle_count = mPF_option.particles_num;

	setUniformRandomGenerater(max_x, min_x, max_y, min_y,max_z, min_z);
	
	for (size_t i = 0; i < mPF_option.particles_num; i++) {
		Particle<Pose3D> particle;
		particleInitInGlobalMap(particle.data, ground_truth[0].yaw, ground_truth[0].pitch, ground_truth[0].roll);
		m_particles.push_back(particle);
	}
	
}

void ParticleFilter::pfOptionInit(PF_option pf_option)
{
	this->mPF_option = pf_option;
	if (this->mPF_option.particles_num==0) {
		this->mPF_option.particles_num = 200;
		LogCat::c(TAG, "pfOptionInit--particle count", "particles_num=0!!! modify the localization.ini");
	}
	LogCat::c(TAG, "particleDataInit--particle=",to_string(mPF_option.particles_num)+ "\n shift_particle_function="+to_string(mPF_option.shift_particle_function));
}
//*************************************************************************************************
//*���������
//*
void ParticleFilter::executeOn(const Pose3D & action, const pcl::PointCloud<PointIoType>::Ptr observation, MPointCloudMap& map)
{
	// 1,2) Prediction & Update stages:
	// ---------------------------------------------------
	prediction_and_update(action,observation,map);
	// 3) Resample & Normalize weights:
	// ---------------------------------------------------
	
	normalizeWeights();

	// Save weights statistics
	// ---------------------------------------------------
	const size_t M = m_particles.size();
	// ESS:
	PF_status.ESS_beforeResample = ESS();
	LogCat::c("ParticleFilter", "Before Resampling particles ESS = ", PF_status.ESS_beforeResample);
	// Variance:
	if (M > 1)
	{
		double weightsMean = 0, var = 0;
		for (size_t i = 0; i < M; i++) weightsMean += exp(m_particles[i].log_w);
		weightsMean /= M;
		for (size_t i = 0; i < M; i++)
			var += pow(exp(m_particles[i].log_w) - weightsMean, 2);
		var /= (M - 1);
		PF_status.weightsVariance_beforeResample = var;
	}

	// 4) Particles resampling stage
	// ---------------------------------------------------
	vector<size_t> indxs;
	MRPTRsample(indxs);
	
	saveResampleCovarianceAndMean();
	
	//5) Particle shift
	if (mPF_option.doParticleShift == 1) {
		//�������shift particle�����������Զ�Ӧ�ز���������Ӽ�
		std::sort(indxs.begin(), indxs.end());
		//doParticleShift(observation, map);
		doParitcleShiftWithResampleIndex(indxs,observation,map);
		//doParticleShift(observation, map);
	}

	saveShiftCovarianceAndMean();
}


void ParticleFilter::prediction_and_update(const Pose3D & action, const pcl::PointCloud<PointIoType>::Ptr observation, MPointCloudMap& map)
{
	TRY_START
	
	// --------------------------------------------------------------------------------------
	//  Prediction: samples from the motion model and update particles
	// --------------------------------------------------------------------------------------
	MyMath mMymath;
	
	const size_t M = m_particles.size();
	for (size_t i = 0; i < M;i++) {
		Pose3D addition = action;
		mMymath.poseDisturbance(addition,mPF_option.noisy);
		Pose3D finalPose = Pose3D(getLastPose(i) + addition);
		updateParticleWithNewPose(m_particles[i].data,finalPose);
	}
	ParticlesData::Particles shift_particles;
	predictionDone(shift_particles);

	saveGuassCovarianceAndMean();

	// --------------------------------------------------------------------------------------
	//  LikelihoodCompute: Compute all the likelihood values & update particles weight:
	// --------------------------------------------------------------------------------------
	if (observation) {
		//do particle shift likelihood compute
		
		if (mPF_option.doParticleShift) {
			map.clearCorrespondenceCloudPoint();
			for (size_t i = 0; i < M; i++) {
				Pose3D lastPose = getLastPose(i);
				const double obs_log_lik = map.likelihoodComputeWithPointCloudSave(m_particles[i].data, observation);
				m_particles[i].log_w = obs_log_lik;
				LogCat::h("ParticleFilter", "prediction_and_update", obs_log_lik);
			}
		}
		else {
			for (size_t i = 0; i < M; i++) {
				Pose3D lastPose = getLastPose(i);
				const double obs_log_lik = map.likelihoodCompute(m_particles[i].data, observation);
				m_particles[i].log_w = obs_log_lik;
				LogCat::h("ParticleFilter", "prediction_and_update", obs_log_lik);
			}	
		}
		
	}
	TRY_END
}

void ParticleFilter::MRPTRsample(vector<size_t>& indxs)
{
	// Make a vector with the particles' log. weights:
	const size_t in_particle_count = m_particles.size();
	//vector<size_t> indxs;
	vector<double> log_ws;
	log_ws.assign(in_particle_count, .0);
	for (size_t i = 0; i < in_particle_count; i++) log_ws[i] = m_particles[i].log_w;
	// Compute the surviving indexes:
	MRPTComputeResampling(log_ws, indxs);

	// And perform the particle replacement:
	MRPTperformSubstitution(indxs);

	// Finally, equal weights:
	/*for (size_t i = 0; i < in_particle_count; i++) {
		m_particles[i].log_w=0;
	}*/
		
}
void ParticleFilter::MRPTComputeResampling(const vector<double>& in_logWeights, vector<size_t>& out_indexes)
{
	// Compute the normalized linear weights:
	//  The array "linW" will be the input to the actual
	//  resampling algorithms.
	size_t i, j, M = in_logWeights.size();

	vector<double> linW(M, 0);
	vector<double>::const_iterator inIt;
	vector<double>::iterator outIt;
	double linW_SUM = 0;

	// This is to avoid float point range problems:
	double max_log_w = 0;
	for (auto& p:in_logWeights) {
		max_log_w = std::max<double>(max_log_w,p);
	}
	for (i = 0, inIt = in_logWeights.begin(), outIt = linW.begin(); i < M;
		i++, inIt++, outIt++)
		linW_SUM += ((*outIt) = exp((*inIt) - max_log_w));
	// Normalize weights:
	const size_t N = linW.size();
	for (size_t i = 0; i < N; i++) linW[i] *= 1.0 / linW_SUM;

	// ==============================================
	//   Select with replacement
	// ==============================================
	vector<double> Q(M);
	cumsum_tmpl(linW,Q);
	Q[M - 1] = 1.1;
	vector<double> T(M);
	T = GenerateRandomVector(M);
	T.push_back(1.0);
	// Sort:
		// --------------------
	sort(T.begin(), T.end());

	out_indexes.resize(M);
	i = j = 0;

	while (i < M)
	{
		if (T[i] < Q[j])
		{
			out_indexes[i++] = (unsigned int)j;
		}
		else
		{
			j++;
			if (j >= M) j = M - 1;
		}
	}
}

/** Replaces the old particles by copies determined by the indexes in
	 * "indx", performing an efficient copy of the necesary particles only and
	 * allowing the number of particles to change.*/
void ParticleFilter::MRPTperformSubstitution(const std::vector<size_t>& indx)
{
		// Ensure input indices are sorted
	std::vector<size_t> sorted_indx(indx);
	

	std::sort(sorted_indx.begin(), sorted_indx.end());

	/* Temporary buffer: */
	Particles parts;
	parts.resize(sorted_indx.size());
	// Implementation for particles as values:
	auto it_idx = sorted_indx.begin();
	auto itDest = parts.begin();
	for (; itDest != parts.end(); ++it_idx, ++itDest)
		*itDest = m_particles[*it_idx];

	/* Move particles to the final container: */
	m_particles = std::move(parts);
}

//load the particle weight and normalize
void ParticleFilter::normalizeWeights()
{
	if (m_particles.size() == 0) return;
	double min_weight = m_particles[0].log_w;
	double max_weight = min_weight;

	//Compute the max/min of weights
	for (int i = 0; i < m_particles.size(); i++) {
		min_weight = std::min<double>(min_weight,m_particles[i].log_w);
		max_weight = std::max<double>(max_weight,m_particles[i].log_w);
	}
	// normalize
	//double sum = GetSumOfParticle();
	for (int i = 0; i < m_particles.size(); i++) {
		//double x = (m_particles[i].log_w / sum);
		m_particles[i].log_w -= max_weight;
	}
}

//��������

void ParticleFilter::doParticleShift(const pcl::PointCloud<PointIoType>::Ptr observation,MPointCloudMap& map)
{
	ParticlesData::Particles shift_particles;
	predictionDone(shift_particles);
	for (size_t i = 0; i <m_particles.size(); i++) {
		Pose3D lastPose = getLastPose(i);
		Pose3D shift_pose = map.likelihoodComputeWithParticleShift(m_particles[i].data, observation, mPF_option.shift_particle_function);
		shift_particles.push_back(Particle<Pose3D>(m_particles[i].log_w, shift_pose));
		//const double obs_log_lik = map.likelihoodCompute(shift_pose, observation);
		m_particles[i].data = shift_pose;
		//m_particles[i].log_w = 0;
		//m_particles[i].log_w = obs_log_lik;
		//m_particles[i].data = _shift_particle;
		LogCat::h("ParticleFilter", "prediction_and_update", "Shift particle");
	}
	predictionDone(shift_particles);
}

void ParticleFilter::doParitcleShiftWithResampleIndex(vector<size_t>& indexes, const pcl::PointCloud<PointIoType>::Ptr observation, MPointCloudMap & map)
{
	ParticlesData::Particles shift_particles;
	predictionDone(shift_particles);
	
	for (size_t i = 0; i < m_particles.size(); i++) {
		
		Pose3D shift_pose = map.doComputeWithParticleShift(m_particles[i].data, indexes[i], observation, mPF_option.shift_particle_function);
		shift_particles.push_back(Particle<Pose3D>(m_particles[i].log_w, shift_pose));
		//const double obs_log_lik = map.likelihoodCompute(shift_pose, observation);
		m_particles[i].data = shift_pose;
		//m_particles[i].log_w = 0;
		//m_particles[i].log_w = obs_log_lik;
		//m_particles[i].data = _shift_particle;
		LogCat::h("ParticleFilter", "prediction_and_update", "Shift particle");
	}
	
	predictionDone(shift_particles);
}



void ParticleFilter::setParticle(int M)
{
	for (int i = 0; i < M; i++) {
		m_particles[i].data = Pose3D(i, i + 1, i + 2, i + 3, i + 4, i + 5);
		m_particles[i].log_w = i;
	}
}


double ParticleFilter::ESS()
{
	TRY_START
		double cum = 0;

	/* Sum of weights: */
	double sumLinearWeights = 0;
	for (auto it = m_particles.begin();
		it != m_particles.end(); ++it)
		sumLinearWeights += std::exp(it->log_w);
	/* Compute ESS: */
	for (auto it = m_particles.begin();
		it != m_particles.end(); ++it)
		cum += pow(std::exp(it->log_w) / sumLinearWeights, 2);

	if (cum == 0)
		return 0;
	else
		return 1.0 / (m_particles.size() * cum);
	TRY_END
}

void ParticleFilter::logOutPut(string root, vector<int> using_time, vector<Pose3D> pose_list)
{
	string time_output = root + "timeusing.txt";
	ofstream timefout;
	timefout.open(time_output);//
	for (auto& t : using_time) {
		timefout << t << endl;
	}
	timefout.close();

	string pose_output = root + "gtpose.txt";
	ofstream posefout;
	posefout.open(pose_output);//
	for (int i = 0; i < pose_list.size(); i++) {
		posefout << pose_list[i].x << " " << pose_list[i].y << " " << pose_list[i].z << " " << pose_list[i].yaw <<endl;
	}
	posefout.close();
}


void ParticleFilter::cumsum_tmpl(vector<double>& in_data, vector<double>& out_cumsum) {
	double last = 0;
	const size_t N = in_data.size();
	for (size_t i = 0; i < N; i++) last = out_cumsum[i] = last + in_data[i];

}

//******************************************bad code***************************************
void ParticleFilter::doResample()
{
	vector<double> orign, output, output_temp, p_weights, weight_temp;
	//load the particle weight and normalize
	if (m_particles.size() < 0) throw "there is no particles to resample";
	/*double max_weight = getMaxWeight();*/
	double sum_weight = 0.0;
	/*for (auto &p:m_particles) {
		sum_weight += exp(p.log_w-max_weight);
	}*/
	//if (sum_weight<=0) throw "can not resample";
	double exp_l = 0.0;
	for (int i = 0; i < m_particles.size(); i++) {
		orign.push_back(m_particles[i].log_w);
		exp_l = exp(-m_particles[i].log_w);
		p_weights.push_back(exp_l);
		sum_weight += exp_l;
	}
	int iterate = 1;
	LogCat::h("ParticleFilter", "doResample--orign:", orign);
	LogCat::c("ParticleFilter", "m_particles num = ", m_particles.size());
	vector<int> index(p_weights.size(), 0);
	output = orign;
	weight_temp.assign(p_weights.begin(), p_weights.end());
	for (auto& w : weight_temp) {
		w = w / sum_weight;
	}
	Particles resampled_particles;
	for (int i = 0; i < iterate; i++) {
		resampled_particles.clear();
		index.clear();
		index = resample_Multinomial(weight_temp);
		for (int j = 0; j < orign.size(); j++) {
			//resample
			resampled_particles.push_back(m_particles[index[j]]);
			//m_particles[j] = m_particles[index[j]];
			;			output_temp.push_back(output[index[j]]);
		}
		output = output_temp;
		output_temp.clear();
		double sum = 0;
		for (auto&p : output) {
			sum += p;
		}
		weight_temp.clear();
		for (int k = 0; k < output.size(); k++) {
			weight_temp.push_back(double(output[k] / sum));
		}
		LogCat::h("ParticleFilter", "doResample--output weight:", output);
		//DisplayVector(weight_temp);
	}
	//accept the resampled particles and set the weight =0
	m_particles = resampled_particles;
	for (auto p : m_particles) {
		p.log_w = 0;
	}
}
//******************************************bad code***************************************
vector<int> ParticleFilter::resample_Multinomial(const vector<double>& weights)
{
	size_t size = weights.size();
	vector<int> N_babies(size);
	vector<double> cdf(size);
	//DisplayVector(weights);
	cdf[0] = weights[0];

	for (int i = 1; i < size; i++) {
		cdf[i] = weights[i] + cdf[i - 1];
	}
	vector<double> u(size), uu(size);
	u = GenerateRandomVector(size);
	for (int i = 0; i < size; i++) {
		uu[i] = pow(u[i], double(1.0 / (size - i)));
	}
	vector<double> multiplie(size);
	multiplie = multiplier(uu, size);
	reverse(multiplie.begin(), multiplie.end());
	//assign multiplie to u
	u.swap(multiplie);
	int j = 0;//
	//*************log********************
	//DisplayVector(cdf);
	for (int i = 0; i < size; i++) {
		while (u[i] > cdf[j]) {
			j += 1;
		}
		N_babies[j] += 1;
	}
	int index = 0;
	vector<int> OutIndex(size);
	for (int i = 0; i < size; i++) {
		if (N_babies[i] > 0) {
			for (int k = index; k < index+N_babies[i]; k++) {
				OutIndex[k] = i;
			}
		}
		index = index + N_babies[i];
	}
	return OutIndex;
}
//******************************************bad code***************************************
//srand(1) if not use srand,then the random number generating 
//form rand while be the same at every running
void ParticleFilter::resample_Wheel()
{
	//generate a seed for rand
	std::default_random_engine gen;
	//load the particle weight
	vector<double> weights;
	for (auto& p: m_particles) {
		weights.push_back(p.log_w);
	}
	// Creating distributions.
	uniform_real_distribution<float> dist_float(0.0, max_weight);
	uniform_int_distribution<int> dist_int(0, mInitPara.particle_count - 1);
	// Generating index.
	int index = dist_int(gen);
	double beta = 0.0;
	Particles resampled_particles;
	//repeat resample
	cout << "the display of WheelResample:" << endl;
	for (int j = 1; j < 2;j++) {
		resampled_particles.clear();
		for (int i = 0; i < mInitPara.particle_count; i++) {
			beta += dist_float(gen) * 2.0;//if want to use the wheelresample ,the para i think should be change
										  //otherwise the particles will only remain one after resampled 
			while (beta > weights[index]) {
				beta -= weights[index];
				index = (index + 1) % mInitPara.particle_count;
			}
			resampled_particles.push_back(m_particles[index]);
		}
		m_particles = resampled_particles;
		DisplayParticleWeight();
	}
}





